# import numpy as np
# g = 9.81  # 重力加速度 (m/s²)
# R = 10.0  # 烟幕有效遮蔽半径 (m)
# T = np.array([0, 200, 0])  # 真目标中心（圆柱体中心）
# Of = np.array([0, 0, 0])  # 假目标（原点）
#
# # 导弹参数（M1）
# M0 = np.array([20000, 0, 2000])  # 导弹初始位置
# uM = (Of - M0) / np.linalg.norm(Of - M0)  # 导弹航向单位向量（指向假目标）
# vM = 300 * uM  # 导弹速度向量 (m/s)
#
# # 无人机参数（FY1）
# F0 = np.array([17800, 0, 1800])  # 无人机初始位置
# vF = np.array([-120, 0, 0])  # 无人机速度向量（等高度匀速）(m/s)
# t_rel = 1.5  # 无人机受领任务后，烟幕弹投放延迟时间 (s)
# D = F0 + vF * t_rel  # 烟幕弹投放点（脱离无人机时的位置）
#
# # 烟幕弹起爆参数
# t_det_rel = 3.6  # 投放后到起爆的延迟时间 (s)
# t_det = t_rel + t_det_rel  # 全局起爆时刻（t=0为任务下达）
#
# # 计算起爆点位置（考虑投放后自由下落）
# x_det = D[0] + vF[0] * t_det_rel  # 起爆点x坐标
# y_det = 0  # 起爆点y坐标（与无人机同y平面）
# z_det = 1800 - 0.5 * g * t_det_rel**2  # 起爆点z坐标（自由下落计算）
# C0 = np.array([x_det, y_det, z_det])  # 起爆瞬时云团中心
#
# v_sink = 3  # 起爆后云团中心竖直下沉速度 (m/s)
# valid_window = 20  # 起爆后烟幕有效持续时间 (s)
#
# dt = 1e-3  # 时间步长 (s)
# tL = t_det  # 采样起始时刻（起爆时刻）
# tR = t_det + valid_window  # 采样结束时刻（起爆后20s）
# ts = np.arange(tL, tR + dt, dt).reshape(-1, 1)  # 时间序列（列向量）
#
# # 1. 导弹位置随时间变化 M(t)
# Mx = M0[0] + vM[0] * ts
# My = M0[1] + vM[1] * ts
# Mz = M0[2] + vM[2] * ts
#
# # 2. 云团中心位置随时间变化 C(t)
# Cx = x_det * np.ones_like(ts)
# Cy = np.zeros_like(ts)
# Cz = z_det - v_sink * (ts - t_det)
#
# # 视线向量（导弹指向真目标）u = T - M(t)
# ux = T[0] - Mx
# uy = T[1] - My
# uz = T[2] - Mz
# uu = ux**2 + uy**2 + uz**2  # 视线向量模长的平方
#
# # 云团中心到导弹的向量 w = C(t) - M(t)
# wx = Cx - Mx
# wy = Cy - My
# wz = Cz - Mz
#
# # 向量点积 w·u
# wu = wx * ux + wy * uy + wz * uz
#
# # 投影参数λ（夹在[0,1]之间，确保最近点在线段上）
# lam = wu / uu
# lam = np.maximum(0, np.minimum(1, lam))
#
# # 视线段上的最近点 P*(t)
# Px = Mx + lam * ux
# Py = My + lam * uy
# Pz = Mz + lam * uz
#
# # 云团中心到最近点的距离 d(t)
# dx = Cx - Px
# dy = Cy - Py
# dz = Cz - Pz
# d = np.sqrt(dx**2 + dy**2 + dz**2)
#
# # 遮蔽判据：g(t) = d(t) - R（≤0时表示有效遮蔽）
# gval = d - R
# mask = (gval <= 0)
#
# diff_mask = np.diff(mask.astype(int), axis=0)  # 状态变化差分
# idx_enter = np.where(diff_mask == 1)[0]  # 进入遮蔽的索引
# idx_exit = np.where(diff_mask == -1)[0]   # 离开遮蔽的索引
#
# t_enter = []  # 进入遮蔽的时刻列表
# t_exit = []   # 离开遮蔽的时刻列表
#
# # 1. 计算进入遮蔽的时刻（线性插值）
# for i in idx_enter:
#     t0 = ts[i, 0]
#     t1 = ts[i+1, 0]
#     g0 = gval[i, 0]
#     g1 = gval[i+1, 0]
#     t_star = t0 - g0 * (t1 - t0) / (g1 - g0)
#     t_enter.append(t_star)
#
# # 2. 计算离开遮蔽的时刻（线性插值）
# for i in idx_exit:
#     t0 = ts[i, 0]
#     t1 = ts[i+1, 0]
#     g0 = gval[i, 0]
#     g1 = gval[i+1, 0]
#     t_star = t0 - g0 * (t1 - t0) / (g1 - g0)
#     t_exit.append(t_star)
#
# # 3. 处理首尾时刻处于遮蔽的情况
# if len(mask) > 0 and mask[0, 0]:
#     t_enter.insert(0, tL)
# if len(mask) > 0 and mask[-1, 0]:
#     t_exit.append(tR)
#
# # 4. 对齐遮蔽区间（确保进入和离开时刻一一对应）
# m = min(len(t_enter), len(t_exit))
# t_enter = np.array(t_enter[:m])
# t_exit = np.array(t_exit[:m])
#
# dur_total = 0.0  # 总有效遮蔽时长
# print("=" * 60)
# print("                烟幕遮蔽效果仿真结果")
# print("=" * 60)
# print("——有效遮蔽区间（全局时间，单位：s）——")
# for k in range(m):
#     seg = max(0, t_exit[k] - t_enter[k])
#     dur_total += seg
#     print(f"区间 {k+1:2d}: 起始 = {t_enter[k]:.6f} | 结束 = {t_exit[k]:.6f} | 时长 = {seg:.6f} s")

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.rcParams['font.sans-serif'] = ['SimHei']  # 设置中文字体
plt.rcParams['axes.unicode_minus'] = False  # 解决负号显示问题
# ============ 参数设置 ============
g = 9.81
R = 10.0
T = np.array([0, 200, 0])
Of = np.array([0, 0, 0])
M0 = np.array([20000, 0, 2000])
uM = (Of - M0) / np.linalg.norm(Of - M0)
vM = 300 * uM
F0 = np.array([17800, 0, 1800])
vF = np.array([-120, 0, 0])
t_rel = 1.5
D = F0 + vF * t_rel
t_det_rel = 3.6
t_det = t_rel + t_det_rel
x_det = D[0] + vF[0] * t_det_rel
y_det = 0
z_det = 1800 - 0.5 * g * t_det_rel**2
C0 = np.array([x_det, y_det, z_det])
v_sink = 3
valid_window = 20
dt = 1e-3

# ============ 计算时间序列 ============
tL = t_det
tR = t_det + valid_window
ts = np.arange(tL, tR + dt, dt)
N = len(ts)

# 导弹轨迹
Mx = M0[0] + vM[0] * ts
My = M0[1] + vM[1] * ts
Mz = M0[2] + vM[2] * ts

# 云团中心轨迹
Cx = x_det * np.ones(N)
Cy = np.zeros(N)
Cz = z_det - v_sink * (ts - t_det)

# 视线向量
ux = T[0] - Mx
uy = T[1] - My
uz = T[2] - Mz
uu = ux**2 + uy**2 + uz**2

# 云团到导弹向量
wx = Cx - Mx
wy = Cy - My
wz = Cz - Mz
wu = wx*ux + wy*uy + wz*uz

# 计算最近点参数
lam = wu / uu
lam = np.clip(lam, 0, 1)

# 最近点坐标
Px = Mx + lam * ux
Py = My + lam * uy
Pz = Mz + lam * uz

# 距离计算
dx = Cx - Px
dy = Cy - Py
dz = Cz - Pz
d = np.sqrt(dx**2 + dy**2 + dz**2)

# 有效遮蔽判断
gval = d - R
mask = (gval <= 0)

# 找到遮蔽区间
diff_mask = np.diff(mask.astype(int))
idx_enter = np.where(diff_mask == 1)[0]
idx_exit = np.where(diff_mask == -1)[0]

t_enter = []
t_exit = []

for i in idx_enter:
    t0 = ts[i]
    t1 = ts[i+1]
    g0 = gval[i]
    g1 = gval[i+1]
    t_star = t0 - g0 * (t1 - t0) / (g1 - g0)
    t_enter.append(t_star)

for i in idx_exit:
    t0 = ts[i]
    t1 = ts[i+1]
    g0 = gval[i]
    g1 = gval[i+1]
    t_star = t0 - g0 * (t1 - t0) / (g1 - g0)
    t_exit.append(t_star)

if len(mask) > 0 and mask[0]:
    t_enter = [tL] + t_enter
if len(mask) > 0 and mask[-1]:
    t_exit = t_exit + [tR]

t_enter = np.array(t_enter)
t_exit = np.array(t_exit)
m = min(len(t_enter), len(t_exit))
t_enter = t_enter[:m]
t_exit = t_exit[:m]

# 计算总遮蔽时长
dur_total = 0
print("——有效遮蔽区间（全局时间，s）——")
for k in range(m):
    seg = max(0, t_exit[k] - t_enter[k])
    dur_total += seg
    print(f"区间 {k+1:2d}: [{t_enter[k]:.9f} , {t_exit[k]:.9f}] 时长 = {seg:.9f} s")

print(f"\n 总有效遮蔽时长Δt = {dur_total:.9f} s")
print(f" 起爆点 C0 = ({C0[0]:.3f}, {C0[1]:.3f}, {C0[2]:.4f}) m, 起爆时刻 t_det = {t_det:.1f} s")

# 找到最长遮蔽区间中点
if m >= 1:
    seg_len = t_exit - t_enter
    idx_long = np.argmax(seg_len)
    t_mid = 0.5 * (t_enter[idx_long] + t_exit[idx_long])
else:
    t_mid = t_det + 3.0

ij = np.argmin(np.abs(ts - t_mid))
Mmid = np.array([Mx[ij], My[ij], Mz[ij]])
Cmid = np.array([Cx[ij], Cy[ij], Cz[ij]])
Pmid = np.array([Px[ij], Py[ij], Pz[ij]])

# ============ 可视化 ============
fig = plt.figure(figsize=(15, 10))
fig.suptitle('导弹遮蔽效果分析', fontsize=16, fontweight='bold')

# 1. 距离-时间图
ax1 = fig.add_subplot(2, 2, 1)
ax1.plot(ts, d, 'b-', linewidth=2, label='到视线段最小距离 d(t)')
ax1.axhline(y=R, color='r', linestyle='--', linewidth=2, label='阈值 R=10 m')
ax1.grid(True, alpha=0.3)

# 高亮有效遮蔽区间
yl = ax1.get_ylim()
for k in range(m):
    ax1.axvspan(t_enter[k], t_exit[k], alpha=0.3, color='lightgreen')
    ax1.axvline(x=t_enter[k], color='g', linestyle='-', linewidth=1, alpha=0.7)
    ax1.axvline(x=t_exit[k], color='r', linestyle='-', linewidth=1, alpha=0.7)

ax1.set_xlabel('时间 t (s)')
ax1.set_ylabel('距离 d(t) (m)')
ax1.set_title('距离-时间：遮蔽区间高亮')
ax1.legend()
ax1.set_xlim([tL, tR])

# 2. 导弹和云团轨迹
ax2 = fig.add_subplot(2, 2, 2)
ax2.plot(ts, Mz, 'r-', label='导弹高度')
ax2.plot(ts, Cz, 'b-', label='云团高度')
ax2.set_xlabel('时间 (s)')
ax2.set_ylabel('高度 (m)')
ax2.set_title('高度变化')
ax2.legend()
ax2.grid(True, alpha=0.3)

# 3. XY平面投影
ax3 = fig.add_subplot(2, 2, 3)
ax3.plot(Mx, My, 'r-', label='导弹轨迹')
ax3.plot(Cx, Cy, 'b-', label='云团轨迹')
ax3.scatter(T[0], T[1], color='green', marker='^', s=100, label='真目标')
ax3.scatter(Of[0], Of[1], color='orange', marker='x', s=100, label='假目标')
ax3.scatter(C0[0], C0[1], color='purple', marker='*', s=150, label='起爆点')
ax3.set_xlabel('X (m)')
ax3.set_ylabel('Y (m)')
ax3.set_title('XY平面投影')
ax3.legend()
ax3.grid(True, alpha=0.3)

# 4. 3D视图
ax4 = fig.add_subplot(2, 2, 4, projection='3d')
ax4.plot(Mx, My, Mz, 'r-', label='导弹轨迹', linewidth=2)
ax4.plot(Cx, Cy, Cz, 'b-', label='云团轨迹', linewidth=2)
ax4.scatter(T[0], T[1], T[2], color='green', marker='^', s=100, label='真目标')
ax4.scatter(Of[0], Of[1], Of[2], color='orange', marker='x', s=100, label='假目标')
ax4.scatter(C0[0], C0[1], C0[2], color='purple', marker='*', s=150, label='起爆点')

# 绘制云团球体
u = np.linspace(0, 2 * np.pi, 20)
v = np.linspace(0, np.pi, 20)
x_sphere = Cmid[0] + R * np.outer(np.cos(u), np.sin(v))
y_sphere = Cmid[1] + R * np.outer(np.sin(u), np.sin(v))
z_sphere = Cmid[2] + R * np.outer(np.ones(np.size(u)), np.cos(v))
ax4.plot_surface(x_sphere, y_sphere, z_sphere, alpha=0.2, color='blue')

# 绘制视线
ax4.plot([Mmid[0], T[0]], [Mmid[1], T[1]], [Mmid[2], T[2]], 'g--', label='视线', linewidth=2)
ax4.scatter(Pmid[0], Pmid[1], Pmid[2], color='black', marker='o', s=80, label='最近点')

ax4.set_xlabel('X (m)')
ax4.set_ylabel('Y (m)')
ax4.set_zlabel('Z (m)')
ax4.set_title('3D轨迹视图')
ax4.legend()

plt.tight_layout()
plt.show()

# 额外信息输出
print(f"\n额外信息:")
print(f"导弹初始位置: {M0}")
print(f"导弹速度: {vM}")
print(f"UAV初始位置: {F0}")
print(f"UAV速度: {vF}")
print(f"投放点 D: {D}")
print(f"最长遮蔽区间: [{t_enter[idx_long]:.3f}, {t_exit[idx_long]:.3f}] s")
